/*
 *  MotorController_test.cpp
 *
 *  Created by: Kellen Carey
 *  Date: 11/14/2013
 */

#include "MotorController.h"
#include <iostream>
#include <string>
#include <time.h>
#include <sstream>

using namespace std;

MotorController motorController;

string input;

void motionTesting();

int main(int argc, char* argv[]) {

	motorController.setMotion(STAND);
	motorController.step(false);


	bool done = false;

	do {
		cout << " Enter \"W\" for Walk\n \"Q\" for Left turn\n \"E\" for Right turn\n \"S\" for Stand\n \"A\" for side step left\n \"D\" for side step right\n \"K\" for kick\n \"X\" will exit. " << std::endl;
		std::cout << "Or T for individual testing";

		cin >> input;

		if (input == "W" || input == "w") {
			motorController.setMotion(WALK);
			cout << "Walking" << endl;
		}
		else if (input == "Q" || input == "q") {
			motorController.setMotion(TURNLEFT);
			cout << "Turning Left" << endl;
		}
		else if (input == "E" || input == "e") {
			motorController.setMotion(TURNRIGHT);
			cout << "Turning Right" << endl;
		}
		else if (input == "S" || input == "s") {
			motorController.setMotion(STAND);
			cout << "Standing" << endl;
		}
		else if (input == "A" || input == "a") {
			motorController.setMotion(SIDESTEPLEFT);
			cout << "Stepping Left" << endl;
		}
		else if (input == "D" || input == "d") {
			motorController.setMotion(SIDESTEPRIGHT);
			cout << "Stepping Right" << endl;
		}
		else if (input == "K" || input == "k") {
			motorController.setMotion(KICK);
			cout << "Kicking (left foot)" << endl;
		}
		else if (input == "X" || input == "x"){
			done = true;
		}
		else if (input == "T" || input == "t") {
			motionTesting();
		}
		else {
			cout << "Please enter a valid input" << endl;
		}
		clock_t startTime = clock();

		while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 7 && !done) {
			motorController.step(false);
		}

	} while (!done);

	return 0;
}

void motionTesting() {
	bool FinishMotionTest= false;
	bool motionTestingDone = false;
	while(!FinishMotionTest) {

		motorController.lockHead();
		motionTestingDone = false;

		std::cout << "We are now testing individual motions, with the ability to transfer between steps" << std::endl;
		std::cout << "Which motion would you like to test?" << std::endl;
		cout << " Enter \"W\" for Walk\n \"Q\" for Left turn\n \"E\" for Right turn\n \"S\" for Stand\n \"A\" for side step left\n \"D\" for side step right\n \"K\" for kick\n \"X\" will exit. ";


		cin >> input;

		if (input == "W" || input == "w")
		{
			motorController.setMotion(WALK);
		}
		else if (input == "Q" || input == "q")
		{
			motorController.setMotion(TURNLEFT);
		}
		else if (input == "E" || input == "e")
		{
			motorController.setMotion(TURNRIGHT);
		}
		else if (input == "S" || input == "s")
		{
			motorController.setMotion(STAND);
		}
		else if (input == "A" || input == "a")
		{
			motorController.setMotion(SIDESTEPLEFT);
		}
		else if (input == "D" || input == "d")
		{
			motorController.setMotion(SIDESTEPRIGHT);
		}
		else if (input == "K" || input == "k")
		{
			motorController.setMotion(KICK);
		}
		else if (input == "X" || input == "x"){
			return;
		}

		else{
			cout << "Please enter a valid input" << endl;
			motionTestingDone = true;
		}


		while (!motionTestingDone) {
			std::cout << "n -> Next step "<<std::endl;
//			std::cout << "p -> Previous step  "<<std::endl;
			std::cout << "r -> Release *SINGLE* Motor  "<<std::endl;
			std::cout << "ra -> Release *ALL* Motors  "<<std::endl;
			std::cout << "e -> Engage *SINGLE* Motor  "<<std::endl;
			std::cout << "ea -> Engage *ALL* Motors  "<<std::endl;
			std::cout << "f -> Finish the current motion " << std::endl;
			std::cout << "rl -> Release Left Leg " << std::endl;
			std::cout << "rr -> Release Right Leg " << std::endl;
			std::cout << "er -> Enable Right Leg " << std::endl;
			std::cout << "el -> Enable Left Leg " << std::endl;
			std::cout << "rp -> Read Position " << std::endl;

			cin >> input;


			if (input == "n" || input == "N") {

				/**
				 *  If the incrementStep function returns false, then the while loop continues, and 			waits for user input.
				 *  If it returns true, that means the motion has no more steps and the while loop 			exits
				 */
				motionTestingDone= motorController.incrementStep();
				if(motionTestingDone==true) {
					bool validInput = false;
					while (!validInput) {
						std::cout<<"Motion is over. Test another motion?  (Y/N)"<<std::endl;
						cin >> input;
						if(input == "Y" || input == "y") {
							FinishMotionTest= false;
							validInput = true;
						}
						else if(input == "N" || input == "n") {
							FinishMotionTest= true;
							validInput = true;
						}
						else {
							std::cout << "Please enter a valid input" << std::endl;
						}
					}


				}
				//motorController.executeNextStep(motorController.getMotionFile());
			}

//			else if (input == "p" || input == "P") {
//
//				motorController.executePreviousStep(motorController.getMotionFile());
//			}
			else if (input == "r" || input == "R") {
				//release single motor
				std::cout <<"Which motor would you like to release? ";
				std::string tempinput;
				cin >> tempinput;
				int motor = 0;
				std::stringstream(tempinput) >> motor;
				motorController.disableMotor(motor);
			}
			else if (input == "ra" || input == "RA") {
				//release all motors
				motorController.disableAllMotors();
			}
			else if (input == "e" || input == "E") {
				//engage single motor
				std::cout <<"Which motor would you like to engage? ";
								std::string tempinput;
								cin >> tempinput;
								int motor;
								std::stringstream(tempinput) >> motor;
								motorController.enableMotor(motor);
			}
			else if (input == "ea" || input == "EA") {
				//engage all motors
				motorController.enableAllMotors();
			}
			else if (input == "f" || input == "F") {
				clock_t startTime = clock();
				while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 5) {
							motorController.step(false);
						}
				motionTestingDone = true;
			}
			else if (input == "er") {
				motorController.enableRightLeg();
			}
			else if (input == "el") {
				motorController.enableLeftLeg();
			}
			else if (input == "dr") {
				motorController.disableRightLeg();
			}
			else if (input == "dl") {
				motorController.disableLeftLeg();
			}
			else if (input == "rp") {
				std::string tempinput;
				int motor;
				std::cout << "Which motor would you like to read? ";
				std::cin >> tempinput;
				std::stringstream(tempinput)>>motor;
				std::cout << motorController.readMotorPosition(motor) << std::endl;
			}
			else {
				std::cout << "Please enter a valid input" << std::endl;
			}
		}
	}
}
